#include "keypoints.h"
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/search/kdtree.h>
#include "functions.h"


void uniformKeypoints(PointCloud<PointXYZ>::Ptr input,PointCloud<PointXYZI>::Ptr keypoints, double leafSize)
{
    //cout << "Uniform Keypoint detection..." << flush;
    keypoints->clear();
    UniformSampling<PointXYZ>* us= new UniformSampling<PointXYZ>;
    search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> );
    us->setInputCloud(input);
    us->setRadiusSearch(leafSize);
    us->setSearchMethod(tree);
    PointCloud<int>::Ptr temp(new PointCloud<int>);
    us->compute(*temp);
    for (int i=0;i<temp->size();i++)
    {
        PointXYZI point;
        point.x=input->at( temp->at(i)).x;
        point.y=input->at( temp->at(i)).y;
        point.z=input->at( temp->at(i)).z;
        keypoints->push_back(point);
    }
    //cout << "OK! Keypoints found: " << keypoints->points.size() << endl;
}
